﻿using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;
using System.Drawing;

namespace LocalizationTest
{
  class MBLExperimentSpecific : SelfLocator
  {
    public override WorldState Execute(Experiment experiment)
    {
      if (experiment is Experiment2DBearingSensor)
      {
        return DoExperimentSpecificMBL2DBearing((Experiment2DBearingSensor)experiment);
      }
      if (experiment is Experiment2DDistanceSensor)
      {
        return DoExperimentSpecificMBL2DDistance((Experiment2DDistanceSensor)experiment);
      }
      if (experiment is ExperimentPose2DBearings)
      {
        return DoExperimentSpecificMBLPose2DBearing((ExperimentPose2DBearings)experiment);
      }
      return null;
    }

    #region 2D bearing
    private WorldState DoExperimentSpecificMBL2DBearing(Experiment2DBearingSensor experiment)
    {
      int counter = experiment.actionAndObservationHistory.history.Count - 1;
      if (counter == -1) return null;
      Dictionary<int, List<double>> sumOfY = new Dictionary<int, List<double>>();
      List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces = experiment.actionAndObservationHistory.GetSelectedRelativePlaces(experiment.simulatedWorldState);
      foreach (ActionAndObservationHistory.RelativePlaceWithMeasurement entry in listOfOldPlaces)
      {
        WorldState2D worldState = (WorldState2D)entry.worldState;
        //        if (counter < 0) break;
        double x_r = worldState.Vector.x;
        double y_r = worldState.Vector.y;

        if (entry.measurement != null)
        {
          double alpha = ((AngleMeasurement)(entry.measurement)).Angle;
          double measurementInDegrees = alpha * 180.0 / Math.PI;
          if (Math.Abs(measurementInDegrees) < 88 || Math.Abs(measurementInDegrees) > 92)
          {
            int xIndex = 0;
            for (int x = Globals.stateSpaceMinimumX; x <= Globals.stateSpaceMaximumX; x += Globals.rasterWidth)
            {
              double y = CalculateYForX(x_r, y_r, alpha, x, entry.measurement.sensor as BearingSensor);
              if (!sumOfY.ContainsKey(xIndex)) sumOfY[xIndex] = new List<double>();
              sumOfY[xIndex].Add(y);
              DebugDrawingTool.CreateCircle("world:MBLExperimentSpecific:YForX", new Vector2(x, y), Globals.rasterWidth / 8.0, 2, System.Drawing.Color.Black);
              xIndex++;
            }
          }
        }//if (entry.measurement != null)
      }

      if (sumOfY.Count == 0) return null;

      int xCounter = 0;
      float minSumOfDifferences = float.MaxValue;
      float maxSumOfDifferences = 0;
      float minSumX = 0;
      float minSumY = 0;
      for (int x = Globals.stateSpaceMinimumX; x <= Globals.stateSpaceMaximumX; x += Globals.rasterWidth)
      {
        double average = 0;
        foreach (double value in sumOfY[xCounter])
        {
          average += value;
        }
        average /= (double)(sumOfY[xCounter].Count);

        float sumOfDifferences = 0;
        foreach (double value in sumOfY[xCounter])
        {
          sumOfDifferences += (float)(Math.Abs(average - value));
        }

        if (sumOfDifferences > maxSumOfDifferences) maxSumOfDifferences = sumOfDifferences;
        if (sumOfDifferences < minSumOfDifferences)
        {
          minSumOfDifferences = sumOfDifferences;
          minSumX = x;
          minSumY = (float)average;
        }

        DebugDrawingTool.CreateCircle("world:MBLExperimentSpecific:Average", new Vector2(x, average), (sumOfDifferences / maxSumOfDifferences * 10.0), 2, System.Drawing.Color.Brown);

        xCounter++;
      }

      DebugDrawingTool.CreateCircle("world:MBLExperimentSpecific:Position", new Vector2(minSumX, minSumY), 25.0, 3, System.Drawing.Color.Orange);
      return new WorldState2D(minSumX, minSumY);
    }
    public static double CalculateYForX(double x_r, double y_r, double alpha, int x, BearingSensor sensor)
    {
      Map map = new MapWithLandmarks(MapWithLandmarks.MapType.lighthouses);
      double x_l = (map as MapWithLandmarks).XPositionOfLandmark[(int)sensor.landmarkID];
      double y_l = (map as MapWithLandmarks).YPositionOfLandmark[(int)sensor.landmarkID];
      //CALCULATE Y FOR X:
      double x_s = x_l - x - x_r;
      double y_s = x_s * Math.Tan(alpha);
      double y = y_l - y_s - y_r;
      return y;
    }
    #endregion

    #region 2D distance
    Vector2 randomPosition = new Vector2(200, 300);

    private WorldState DoExperimentSpecificMBL2DDistance(Experiment2DDistanceSensor experiment)
    {
      if (DebugKeyTool.IsActive("MBLExperimentSpecific:gd reset random position"))
      {
        Random rnd = new Random();
        randomPosition = new Vector2(rnd.Next(-300, +300), rnd.Next(-300, +300));
      }
      if (DebugKeyTool.IsActive("MBLExperimentSpecific:gd reset at left border"))
      {
        randomPosition = new Vector2(-400, 0);
      }
      if (experiment.actionAndObservationHistory.history.Count == 0) return null;
      List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces = experiment.actionAndObservationHistory.GetSelectedRelativePlaces(experiment.simulatedWorldState);

      Vector2 positionOfLandmark = new Vector2(
        (experiment.map as MapWithSingleLandmark).XPositionOfLandmark[(int)MapWithLandmarks.LandmarkIDs.centerLeftFlag],
        (experiment.map as MapWithSingleLandmark).YPositionOfLandmark[(int)MapWithLandmarks.LandmarkIDs.centerLeftFlag]
        );

      DrawCircles(listOfOldPlaces, positionOfLandmark);

      int xCounter = 0;
      int yCounter = 0;

      double x_l = (experiment.map as MapWithSingleLandmark).XPositionOfLandmark[(int)MapWithLandmarks.LandmarkIDs.centerLeftFlag];
      double y_l = (experiment.map as MapWithSingleLandmark).YPositionOfLandmark[(int)MapWithLandmarks.LandmarkIDs.centerLeftFlag];

      List<Function2D> function = new List<Function2D>();
      double maxSumOfDistances = 0;
      double minSumOfDistances = Double.MaxValue;

      Vector2 position = new Vector2();

      if (DebugKeyTool.IsActive("MBLExperimentSpecific:global minimum function"))
      {
        for (int x = Globals.stateSpaceMinimumX; x <= Globals.stateSpaceMaximumX; x += Globals.rasterWidth)
        {
          for (int y = Globals.stateSpaceMinimumY; y <= Globals.stateSpaceMaximumY; y += Globals.rasterWidth)
          {
            double sumOfDistances = CalculateMatch(listOfOldPlaces, x_l, y_l, x, y);
            function.Add(new Function2D(x, y, sumOfDistances));
            if (sumOfDistances > maxSumOfDistances) maxSumOfDistances = sumOfDistances;
            if (sumOfDistances < minSumOfDistances)
            {
              minSumOfDistances = sumOfDistances;
              position.x = x;
              position.y = y;
            }
            yCounter++;
          }//for y..
          xCounter++;
        }//for x..

        foreach (Function2D entry in function)
        {
          if (maxSumOfDistances >= 1)
          {
            int intensity = (int)(255.0 - entry.value * 255.0 / maxSumOfDistances);
            DebugDrawingTool.CreateLine("world:MBLExperimentSpecific:global minimum function",
            new Vector2(entry.x - Globals.rasterWidth / 2.0, entry.y), new Vector2(entry.x + Globals.rasterWidth / 2.0 + 1, entry.y),
            Globals.rasterWidth + 1, System.Drawing.Color.FromArgb(intensity, intensity, intensity));
          }
        }

        DebugDrawingTool.CreateCircle("world:MBLExperimentSpecific:global minimum position", position, 25, 2, System.Drawing.Color.Green);
      }

      if (DebugKeyTool.IsActive("MBLExperimentSpecific:gd reset with perfect position"))
      {
        randomPosition = position;
      }

      //AVERAGE CENTER TEST
      int numberOfIterations = 20;

      if (DebugKeyTool.IsActive("MBLExperimentSpecific:gd 1 iteration"))
      {
        numberOfIterations = 1;
      }
      if (DebugKeyTool.IsActive("MBLExperimentSpecific:gd 50 iterations"))
      {
        numberOfIterations = 50;
      }

      Vector2 bestPostion;
      if (DebugKeyTool.IsActive("MBLExperimentSpecific:gd inteligent start"))
      {
        Circle circle;
        if (CalculateCircleForLastObservation(listOfOldPlaces, positionOfLandmark, out circle))
        {
          randomPosition = circle.center + new Vector2(circle.radius, 0);
          DoGradientDescent(listOfOldPlaces, positionOfLandmark, numberOfIterations);
          bestPostion = randomPosition;
          double value1 = CalculateMatch(listOfOldPlaces, positionOfLandmark.x, positionOfLandmark.y, randomPosition.x, randomPosition.y);
          randomPosition = circle.center + new Vector2(-circle.radius, 0);
          DoGradientDescent(listOfOldPlaces, positionOfLandmark, numberOfIterations);
          double value2 = CalculateMatch(listOfOldPlaces, positionOfLandmark.x, positionOfLandmark.y, randomPosition.x, randomPosition.y);
          if (value2 < value1) bestPostion = randomPosition;
          randomPosition = position;
          DoGradientDescent(listOfOldPlaces, positionOfLandmark, numberOfIterations);
        }
        else
        {
          DoGradientDescent(listOfOldPlaces, positionOfLandmark, numberOfIterations);
          bestPostion = randomPosition;
        }
      }
      else
      {
        DoGradientDescent(listOfOldPlaces, positionOfLandmark, numberOfIterations);
        bestPostion = randomPosition;
      }
      DebugDrawingTool.CreateCircle("world:MBLExperimentSpecific:gd position", bestPostion, 15, 12, Color.Lime);
      return new WorldState2D(bestPostion.x, bestPostion.y);
    }
    private void DoGradientDescent(List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces, Vector2 positionOfLandmark, int numberOfIterations)
    {
      for (int i = 0; i < numberOfIterations; i++)
      {
        Vector2 oldPosition = randomPosition;
        MoveRandomPosition(listOfOldPlaces, positionOfLandmark);
        int intensity = (int)((numberOfIterations - i) * 200.0 / numberOfIterations);
        Color color = Color.FromArgb(200 - intensity, 0, intensity);
        DebugDrawingTool.CreateLine("world:MBLExperimentSpecific:gd path", oldPosition, randomPosition, 4, color);
      }
    }
    private void MoveRandomPosition(List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces, Vector2 positionOfLandmark)
    {
      Vector2 averageVectorToCircles = new Vector2();
      double maxLength = 0;
      foreach (ActionAndObservationHistory.RelativePlaceWithMeasurement entry in listOfOldPlaces)
      {
        WorldState2D worldState = (WorldState2D)entry.worldState;
        Vector2 relativePosition = new Vector2(worldState.Vector.x, worldState.Vector.y);
        if (entry.measurement != null)
        {
          Vector2 centerOfCircleOnField = positionOfLandmark - relativePosition;
          double radius = ((DistanceMeasurement)(entry.measurement)).Distance;
          Vector2 vectorToCircle = (centerOfCircleOnField - randomPosition);
          //          if (vectorToCircle.Abs() != 0)
          {
            vectorToCircle *= (vectorToCircle.Abs() - radius) / vectorToCircle.Abs();
            if (vectorToCircle.Abs() > maxLength) maxLength = vectorToCircle.Abs();
            averageVectorToCircles += vectorToCircle;
            DebugDrawingTool.CreateLine("world:MBLExperimentSpecific:function calculation", randomPosition, randomPosition + vectorToCircle, 2, System.Drawing.Color.Red);
          }
        }//if (entry.measurement != null)
      }// for each entry in history
      DebugDrawingTool.CreateLine("world:MBLExperimentSpecific:function calculation", randomPosition, randomPosition + averageVectorToCircles, 2, System.Drawing.Color.Blue);
      if (averageVectorToCircles.Abs() != 0)
        randomPosition += averageVectorToCircles / averageVectorToCircles.Abs() * maxLength * 0.1;
    }
    private void DrawCircles(List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces, Vector2 positionOfLandmark)
    {
      foreach (ActionAndObservationHistory.RelativePlaceWithMeasurement entry in listOfOldPlaces)
      {
        WorldState2D worldState = (WorldState2D)entry.worldState;
        Vector2 relativePosition = new Vector2(worldState.Vector.x, worldState.Vector.y);
        if (entry.measurement != null)
        {
          Vector2 centerOfCircleOnField = positionOfLandmark - relativePosition;
          double radius = ((DistanceMeasurement)(entry.measurement)).Distance;
          DebugDrawingTool.CreateCircle("world:MBLExperimentSpecific:constraints", centerOfCircleOnField, radius, 2, System.Drawing.Color.Gray);
        }//if (entry.measurement != null)
      }// for each entry in history
    }
    private bool CalculateCircleForLastObservation(List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces, Vector2 positionOfLandmark, out Circle circle)
    {
      foreach (ActionAndObservationHistory.RelativePlaceWithMeasurement entry in listOfOldPlaces)
      {
        WorldState2D worldState = (WorldState2D)entry.worldState;
        Vector2 relativePosition = new Vector2(worldState.Vector.x, worldState.Vector.y);
        if (entry.measurement != null)
        {
          Vector2 centerOfCircleOnField = positionOfLandmark - relativePosition;
          double radius = ((DistanceMeasurement)(entry.measurement)).Distance;
          circle = new Circle(centerOfCircleOnField, radius);
          DebugDrawingTool.CreateCircle("world:MBLExperimentSpecific:gradient descent circle", centerOfCircleOnField, radius, 2, System.Drawing.Color.Brown);
          return true;
        }//if (entry.measurement != null)
      }// for each entry in history
      circle = new Circle();
      return false;
    }
    private static double CalculateMatch(List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces, double x_l, double y_l, double x, double y)
    {
      double sumOfDistances = 0;
      foreach (ActionAndObservationHistory.RelativePlaceWithMeasurement entry in listOfOldPlaces)
      {
        WorldState2D worldState = (WorldState2D)entry.worldState;
        double x_r = worldState.Vector.x;
        double y_r = worldState.Vector.y;
        if (entry.measurement != null)
        {
          double radius = ((DistanceMeasurement)(entry.measurement)).Distance;
          double distanceToCircle =
            Math.Pow(
            Math.Abs(
            Math.Sqrt(Math.Pow(x - (x_l - x_r), 2) + Math.Pow(y - (y_l - y_r), 2))
              - radius), 0.5);
          sumOfDistances += distanceToCircle;
        }//if (entry.measurement != null)
      }// for each entry in history
      return sumOfDistances;
    }// method
    #endregion

    #region Pose2D Bearing
    private WorldState DoExperimentSpecificMBLPose2DBearing(ExperimentPose2DBearings experimentPose2DBearings)
    {
      for (int x = -500; x < 500; x += 10)
      {
        for (int y = -500; y < 500; y += 10)
        {
          double sumOfDeltas = CalculateFunction(x, y);
          double visualizationFactor = 800.0;
          int color = (int)(255 - sumOfDeltas * visualizationFactor);
          if (color < 0) color = 0;
          DebugDrawingTool.CreateLine("world:MBL:specific", 
            new Vector2(x - 5, y), new Vector2(x + 5, y), 10, 
            Color.FromArgb(color,color,color)
            //Color.FromArgb((x + 500) / 4, (y + 500) / 4, (x + 500) / 4)
            );
//          DebugDrawingTool.CreateLine("world:MBL:specific", new Vector2(x - 5, y), new Vector2(x + 5, y), 10, Color.FromArgb((x + 500) / 4, (y + 500) / 4, (x + 500) / 4));
        }
      }
      return new WorldStatePose2D();
    }


    double CalculateFunction(double x, double y)
    {
//      if (experiment.actionAndObservationHistory.history.Count == 0) return null;
//      List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces = experiment.actionAndObservationHistory.GetSelectedRelativePlaces(experiment.simulatedWorldState, experiment);

      double[] angleRad = new double[10];
      double[] angleRad2 = new double[10]; // when angle is ambiguos
      int[] numberOfSolutions = new int[10];
      double[] d = new double[10];
      double sumOfSinesOfUniqueSolutions = 0;
      double sumOfCosinesOfUniqueSolutions = 0;

      //some error removing definitions:
      int _numberOfSelectedLandmarks = 0;
      double[] _angleSeen = new double[10];
      Pose2D[] _odometrySince = new Pose2D[10];
      Vector2[] _position = new Vector2[10];

      for (int i = 0; i < _numberOfSelectedLandmarks; i++)
      {
        numberOfSolutions[i] = CalculateAngleOfRobotAtGivenPositionBasedOnTheBearingToALandmarkAndTheOdometrySinceTheObservation(
          x, y, _angleSeen[i], _odometrySince[i], _position[i], out angleRad[i], out angleRad2[i], false);
        angleRad[i] = MathTools.Normalize(angleRad[i]);
        if (numberOfSolutions[i] != 0)
        {
          /*
                LINE("modules::BearingOnlySelfLocator::angles",
                  x,y, x + 250 * cos( angleRad[i]), y + 250 * sin (angleRad[i]), 20, Drawings::ps_solid, Drawings::red);
           */
        }
        if (numberOfSolutions[i] == 1)
        {
          sumOfSinesOfUniqueSolutions += Math.Sin(angleRad[i]);// * (double)(500 - _selectedOccurence[i]) ;
          sumOfCosinesOfUniqueSolutions += Math.Cos(angleRad[i]);// * (double)(500 - _selectedOccurence[i]) ;
        }
      }
      double average = Math.Atan2(sumOfSinesOfUniqueSolutions, sumOfCosinesOfUniqueSolutions);
      /*
         */
      double sumOfDeltas = 0;
      for (int i = 0; i < _numberOfSelectedLandmarks; i++)
      {
        double square1 = Math.Pow(MathTools.Normalize(angleRad[i] - average),2);   // * pow((double)(500 - _selectedOccurence[i]), 3);
        double square2 = Math.Pow(MathTools.Normalize(angleRad2[i] - average), 2);
        if (numberOfSolutions[i] == 0) d[i] = Math.Pow(Math.PI,2);
        else if (numberOfSolutions[i] == 1) d[i] = square1;
        else if (numberOfSolutions[i] == 2) d[i] = Math.Min(square1, square2);
        else d[i] = Math.Pow(Math.PI,2); // should never happen;
        sumOfDeltas += d[i];
      }
      //  sumOfDeltas /= pow(500.0, 3);
      return sumOfDeltas;
    }

    int CalculateAngleOfRobotAtGivenPositionBasedOnTheBearingToALandmarkAndTheOdometrySinceTheObservation(
      double x, double y,
      double angleSeen, Pose2D odometry, Vector2 positionOfLandmark,
      out double angle1,
      out double angle2,
      bool draw)
    {
      bool thereWasAProblemWithASine = false;
      bool thereAreTwoSolutions = false;
      double distance = odometry.translation.Abs();


      Vector2 vectorToLandmark = (positionOfLandmark - new Vector2(x, y));
      double distanceToLandmark = vectorToLandmark.Abs();
      double angleToLandmark = vectorToLandmark.GetAngle();
      double alpha = angleSeen - odometry.translation.GetAngle();
      double beta;
      beta = Math.Asin(distance * Math.Sin(alpha) / distanceToLandmark);
      double beta2 = beta;
      if (distance > distanceToLandmark)
      {
        double sineBeta = distance * Math.Sin(alpha) / distanceToLandmark;
        if (Math.Abs(sineBeta) > 1.0)
        {
          /*
        LINE("tools::SelfLocatorTools::test", 
          x,y, 
          x, y+20, 
          20, Drawings::ps_solid, Drawings::blue);
          thereWasAProblemWithASine = true;*/
        }
        else
        {
          /*
        LINE("tools::SelfLocatorTools::test", 
          x,y, 
          x, y+20, 
          20, Drawings::ps_solid, Drawings::red);
           */
        }
        thereAreTwoSolutions = true;
        beta2 = Math.PI - beta;
      }
      double gamma = Math.PI - alpha - beta;
      double gamma2 = Math.PI - alpha - beta2;

      Pose2D poseWhenLandmarkWasSeen = new Pose2D();
      Pose2D poseWhenLandmarkWasSeen2 = new Pose2D();
      double angleToPointWhenLandmarkWasSeen = angleToLandmark + gamma;
      double angleToPointWhenLandmarkWasSeen2 = angleToLandmark + gamma2;

      poseWhenLandmarkWasSeen.translation.x = x + Math.Cos(angleToPointWhenLandmarkWasSeen) * distance;
      poseWhenLandmarkWasSeen.translation.y = y + Math.Sin(angleToPointWhenLandmarkWasSeen) * distance;
      poseWhenLandmarkWasSeen.rotation = GetAngleOfRobotAtGivenPositionBasedOnTheBearingToALandmark(
        poseWhenLandmarkWasSeen.translation, angleSeen, positionOfLandmark);

      poseWhenLandmarkWasSeen2.translation.x = x + Math.Cos(angleToPointWhenLandmarkWasSeen2) * distance;
      poseWhenLandmarkWasSeen2.translation.y = y + Math.Sin(angleToPointWhenLandmarkWasSeen2) * distance;
      poseWhenLandmarkWasSeen2.rotation = GetAngleOfRobotAtGivenPositionBasedOnTheBearingToALandmark(
        poseWhenLandmarkWasSeen2.translation, angleSeen, positionOfLandmark);


      Pose2D robotPose1 = new Pose2D();
      robotPose1.rotation = poseWhenLandmarkWasSeen.rotation + odometry.rotation;
      robotPose1.translation.x = x;
      robotPose1.translation.y = y;

      Pose2D robotPose2 = new Pose2D();
      robotPose2.rotation = poseWhenLandmarkWasSeen2.rotation + odometry.rotation;
      robotPose2.translation.x = x;
      robotPose2.translation.y = y;

      angle1 = robotPose1.rotation;
      angle2 = robotPose2.rotation;

      /*
        if(draw && !thereWasAProblemWithASine)
        {
          OUTPUT(idText, text, "---------------" );
          OUTPUT(idText, text, "angleToLandmark: " << toDegrees(angleToLandmark) );
          OUTPUT(idText, text, "angleSeen: " << toDegrees(angleSeen) );
          OUTPUT(idText, text, "odometry.translation.angle(): " << toDegrees(odometry.translation.angle() ) );
          OUTPUT(idText, text, "alpha: " << toDegrees(alpha) );
          OUTPUT(idText, text, "beta: " << toDegrees(beta) );
          OUTPUT(idText, text, "gamma: " << toDegrees(gamma) );
          OUTPUT(idText, text, "angleToPointWhenLandmarkWasSeen: " << toDegrees(angleToPointWhenLandmarkWasSeen) );


          LINE("tools::SelfLocatorTools::test", x,y, positionOfLandmark.x, positionOfLandmark.y, 50, Drawings::ps_solid, Drawings::red);
          LINE("tools::SelfLocatorTools::test", 
            x,y, 
            poseWhenLandmarkWasSeen.translation.x, 
            poseWhenLandmarkWasSeen.translation.y, 40, Drawings::ps_solid, Drawings::pink);
          LINE("tools::SelfLocatorTools::test", 
            x,y, 
            x + cos(angleToLandmark + gamma) * distance, 
            y + sin(angleToLandmark + gamma) * distance, 10, Drawings::ps_solid, Drawings::skyblue);

          LINE("tools::SelfLocatorTools::test", 
            positionOfLandmark.x,
            positionOfLandmark.y,
            poseWhenLandmarkWasSeen.translation.x, 
            poseWhenLandmarkWasSeen.translation.y, 40, Drawings::ps_solid, Drawings::skyblue);
          CIRCLE("tools::SelfLocatorTools::test", x,y, distance, 30, Drawings::ps_solid, Drawings::yellow);

          {
            ARROW("tools::SelfLocatorTools::test", 
              robotPose1.translation.x, robotPose1.translation.y, 
              robotPose1.translation.x + cos(robotPose1.getAngle()) * 200, 
              robotPose1.translation.y + sin(robotPose1.getAngle()) * 200, 20, Drawings::ps_solid, Drawings::red);
          }
          {
            ARROW("tools::SelfLocatorTools::test", 
              poseWhenLandmarkWasSeen.translation.x, poseWhenLandmarkWasSeen.translation.y, 
              poseWhenLandmarkWasSeen.translation.x + cos(poseWhenLandmarkWasSeen.getAngle()) * 200, 
              poseWhenLandmarkWasSeen.translation.y + sin(poseWhenLandmarkWasSeen.getAngle()) * 200, 20, Drawings::ps_solid, Drawings::yellow);
          }
        }
        */
      if (thereWasAProblemWithASine) return 0;
      if (thereAreTwoSolutions) return 2;
      return 1;
    }

    double GetAngleOfRobotAtGivenPositionBasedOnTheBearingToALandmark(Vector2 positionOnField, double angleToLandmark, Vector2 positionOfLandmark)
    {
      return (positionOfLandmark - positionOnField).GetAngle() - angleToLandmark;
    }


    #endregion
  }
}
